#pragma once
// #autor:yxj
#include "rclcpp/rclcpp.hpp"
#include <agv_peripheral_controller/toolKit.h>
#include <agv_msgs/msg/read_in_puts.hpp>
#include <agv_msgs/msg/read_in_puts_new.hpp>
#include <agv_srvs/srv/outputs_new.hpp>
// 急停
#define EMG_BUTTON "emergencyButton"
// 货物检测开关
#define DETECTOR "detector"

// 升降销
#define UP_LIMIT_1 "upLimit1"
#define DOWN_LIMIT_1 "downLimit1"
#define CONTROL_IO_1 "leftIoControl"
#define UP_LIMIT_2 "upLimit2"
#define DOWN_LIMIT_2 "downLimit2"
#define CONTROL_IO_2 "rightIoControl"
// 液压举升
#define UP_LIMIT_3 "upLimit3"
#define DOWN_LIMIT_3 "downLimit3"
#define CONTROL_IO_3_UP "upControl"
#define CONTROL_IO_3_DOWN "downControl"

// 手动旋钮开关
#define SWITCH_COUNT 4
#define IO_LIFT_UP_SWITCH "ioLiftUpSwitch"
#define IO_LIFT_DOWN_SWITCH "ioLiftDownSwitch"
#define LIFT_UP_SWITCH "liftUpSwitch"
#define LIFT_DOWN_SWITCH "liftDownSwitch"
// 开关索引
enum Switch
{
    ioLiftUp,
    ioLiftDown,
    normalLiftUp,
    normalLiftDown
};

// 开关类型
enum class SwitchType
{
    ioLiftType,
    normalLiftType
};

// 三挡旋钮状态
enum class SwitchState
{
    posLeftUp,
    posMiddle,
    posRightDown,
    posInvalid
};

#define SET_ONE_BIT(x) (((uint64_t)1) << (x))
#define TEST_BIT(x, i) ((x & SET_ONE_BIT(i)) != 0)
#define SET_N_BIT(x, n) (x | ((uint64_t)(1) << n))

// 手自动模式获取比特位(agv_info)
#define MANUL_BIT 4
#define HALF_BIT 22
#define AUTO_ONLINE_BIT 23

namespace agv_peripheral
{
    // 定义输入IO点类型
    using INPUT_IO_TYPE = CDataTimeStamp<agv_msgs::msg::PairTypeInt>;

    // 定义输出控制IO点类型
    // 参数： first:io名字 second:控制输出的电平
    using OUTPUT_IO_TYPE = agv_msgs::msg::PairTypeInt;

    // IO控制输出组
    using IO_GROUP = std::vector<OUTPUT_IO_TYPE>;

    // 检测输入IO信号是否触发
    // 返回：是否触发
    inline bool inputIo(INPUT_IO_TYPE &inputIo)
    {

        bool ret = false;
        // 如果数据有效，判断是否触发
        bool timeOut = inputIo.TimeOut(300);
        bool pressed = inputIo.Get().value;

        if (timeOut)
        {
            DEBUG_ERROR_OUT("inputIo:data time out:key=" << inputIo.Get().key);
            return ret;
        }

        ret = pressed;

        return ret;
    }

    // 单举升类
    class CIoLift
    {
    public:
        // 默认构造
        CIoLift();

        // 默认析构
        ~CIoLift();

        // 初始化
        bool init(rclcpp::Client<agv_srvs::srv::OutputsNew>::SharedPtr&);

        // 举升上升
        bool up();

        // 举升下降
        bool down();

        // 举升停止
        bool stop();

        // 货物装填
        bool loadStatus();

        // 前上限位
        INPUT_IO_TYPE upLimit;

        // 前下限位
        INPUT_IO_TYPE downLimit;

        // 控制IO点
        IO_GROUP ctrl;

    private:
        // 上升到位
        bool upTarget();

        // 下降到位
        bool downTarget();

        // 控制IO点输出 数组：param1:索引  param2:触发
        // 返回：成功与否
        bool outputIo(IO_GROUP outIoGroup);

        // 最后输出io的电平状态
        IO_GROUP m_outIoRecord;

        // 控制IO输出客户端
        ros::ServiceClient m_client;
    };

    // IO举升控制类
    class CIoLifterController
    {
    public:
        // 默认构造函数
        CIoLifterController();

        // 析构函数
        ~CIoLifterController();

        // 是否初始化完成
        bool initFinish();

        // 初始化函数
        bool init();

        // 举升上升
        bool up();

        // 举升下降
        bool down();

        // 举升停止
        bool stop();

        // 货物装填
        bool loadStatus();

        // 急停是否按下
        bool emgPressed();

        // 载货传感器
        bool loadDetector();

        ////////特殊用法：左右升降销手动控制//////
        // 左右升
        bool upLeftRight();
        // 左右降
        bool downLeftRight();
        // 停止
        bool stopLeftRight();

        // 三档开关状态
        SwitchState getSwtichState(SwitchType type);

        // 手动旋钮控制举升
        bool manulSwitchIoLift(bool reset = false);

        bool manulSwitchNormalLift(bool reset = false);

    private:
        // 获取新IO输入信号回调函数
        void readInputNewCallBack(const agv_msgs::msg::ReadInPutsNew &msg);

        // 更新输入IO对象
        void updateInutIoNew(const agv_msgs::msg::ReadInPutsNew &msg, INPUT_IO_TYPE &inputIoObj);

    private:
        // 左举升升降销
        CIoLift m_leftIoLift;

        // 右举升升降销
        CIoLift m_rightIoLift;

        // 液压举升
        CIoLift m_normalLift;

        // 手动旋钮开关
        std::vector<INPUT_IO_TYPE> m_switch;

        // 初始化完成标识
        bool m_init;

        // 私有节点句柄
        std::sherad_ptr<rclcpp::Node> node;

        // 订阅IO输入信号话题
        ros::Subscriber m_subIo;

        // 订阅新的IO输入信号话题
        rclcpp::Subscription<agv_msgs::msg::ReadInPutsNew>::SharedPtr m_subIoNew;

        // 控制IO输出客户端
        rclcpp::Client<agv_srvs::srv::OutputsNew>::SharedPtr m_outClient;

        // 急停信号触发
        INPUT_IO_TYPE m_emgBtn;

        // 货物检测信号触发
        INPUT_IO_TYPE m_detector;
    };
}